/**
 * @file RSVideoCapture.cpp
 * @author 覃雨涛 (1264145445@qq.com)
 * @brief intel realsense深度相机驱动
 * @version 1.0
 * @date 2022-01-23
 *
 * @copyright Copyright SCUT RobotLab(c) 2022
 *
 * @note 内置一个VideoCapture对象实现模组相机打开，尝试
 */

#include "RSVideoCapture.h"
#include <unistd.h>
#include <iostream>

using namespace std;
using namespace rs2;
using namespace cv;

/**
 * @brief 构造函数--REALSENSE相机
 *
 */
RSVideoCapture::RSVideoCapture() : __capture_status(REALSENSE_CAPTURE), __is_inited(true)
{
    this->open();
}

/**
 * @brief 构造函数--模组相机
 * @todo 加入 /dev/video串口号的自动搜索与尝试连接
 * @param serial_num /dev/video<串口号>
 */
RSVideoCapture::RSVideoCapture(uint8_t serial_num)
    : __capture_status(OPENCV_CAPTURE), __serial_num(serial_num), __is_inited(true)
{
    this->open();
    cap->set(CAP_PROP_FRAME_WIDTH, 1280);
    cap->set(CAP_PROP_FRAME_HEIGHT, 720);
    cap->set(CAP_PROP_AUTO_WB, 0);
}

/**
 * @brief 构造函数--打开视频文件，支持bag(rosbag格式文件),mp4、avi
 *
 * @param filename 视频文件名
 */
RSVideoCapture::RSVideoCapture(const string& filename) : __path(filename)
{
    string format = __path.substr(__path.size() - 3, __path.size() - 1);
    if (format == "bag")
    {
        __capture_status = BAGFILE_CAPTURE;
    }
    else if (format == "avi" || format == "mp4")
    {
        __capture_status = VIDEO_CAPTURE;
    }
    else
    {
        ERROR_("Format ERROR!");
        exit(EXIT_FAILURE);
    }
    __is_inited = true;
    this->open();
}

/**
 * @brief 打开相机
 *
 * @return true 打开成功
 * @return false 打开失败
 */
bool RSVideoCapture::open()
{
    /**
     * @brief print Capture status
     */
    string capture_str;
    switch (__capture_status)
    {
        case REALSENSE_CAPTURE:
            capture_str = "REALSENSE_CAPTURE";
            break;
        case BAGFILE_CAPTURE:
            capture_str = "BAGFILE_CAPTURE";
            break;
        case OPENCV_CAPTURE:
            capture_str = "OPENCV_CAPTURE";
            break;
        case VIDEO_CAPTURE:
            capture_str = "VIDEO_CAPTURE";
            break;
    }
    DEBUG_HIGHLIGHT_("Current Status: " << capture_str);

    /**
     * @brief RealSense Capture
     */
    if (__capture_status == REALSENSE_CAPTURE)
    {
        context cxt;
        cxt.query_devices()[0].hardware_reset();
        auto size = (int)cxt.query_devices().size();
        cout << "device num: " << size << endl;
//        sleep(5);
//        size = (int)cxt.query_devices().size();
//        cout << "device num: " << size << endl;
        // -------------------------------- set config --------------------------------
        __config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_BGR8, 60);
        __config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, 60);
        __is_config_set = true;
        // ---------------------------- start pipe profile ----------------------------
        __pipe = make_shared<rs2::pipeline>(cxt);
        __align = make_shared<rs2::align>(RS2_STREAM_COLOR);
        __profile = __pipe->start(__config);
        __is_pipeline_start = true;
        // --------------------------- print revelant Info ---------------------------
        __device = __profile.get_device();

        DEBUG_HIGHLIGHT_("REALSENSE D435 connected");
        DEBUG_HIGHLIGHT_("Serial name: " << __device.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
        DEBUG_HIGHLIGHT_("USB type descriptor is "
                         << __device.get_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR));

        return true;
    }

    /**
     * @brief RosBag Capture
     */
    else if (__capture_status == BAGFILE_CAPTURE)
    {
        // -------------------------------- set config --------------------------------
        __config.enable_device_from_file(__path);
        __is_config_set = true;
        // ---------------------------- start pipe profile ----------------------------
        __pipe = make_shared<pipeline>();
        __align = make_shared<rs2::align>(RS2_STREAM_COLOR);
        __profile = __pipe->start(__config);
        __is_pipeline_start = true;
        // --------------------------- print revelant Info ---------------------------
        __device = __profile.get_device();

        return true;
    }

    /**
     * @brief Camera Capture
     */
    else if (__capture_status == OPENCV_CAPTURE)
    {
        cap = new VideoCapture;
        cap->open(__serial_num);
        return true;
    }
    /**
     * @brief Video Capture
     */
    else if (__capture_status == VIDEO_CAPTURE)
    {
        cap = new VideoCapture;
        cap->open(__path);
        return true;
    }
    ERROR_("初始化失败");
    return false;
}

/**
 * @brief 重写VideoCapture::set函数，设置相机参数
 *
 * @param propId 标识符(EXPOSURE、GAIN、GAMMA、BRIGHTNESS、CONTRAST)
 * @param value 相机参数值
 * @return true 改变成功
 * @return false 改变失败
 */
bool RSVideoCapture::set(int propId, double value)
{
    // [0]是获取depth_sesor，[1]是获取color_sensor
    color_sensor sensor = __device.query_sensors()[1];
    switch (propId)
    {
        case CAP_PROP_EXPOSURE:
            exposure = value;
            sensor.set_option(RS2_OPTION_EXPOSURE, exposure);
            return true;
        case CAP_PROP_GAIN:
            gain = value;
            sensor.set_option(RS2_OPTION_AUTO_GAIN_LIMIT_TOGGLE, 0);
            sensor.set_option(RS2_OPTION_AUTO_GAIN_LIMIT, 0);
            sensor.set_option(RS2_OPTION_GAIN, gain);
            return true;
        case CAP_PROP_GAMMA:
            gamma = value;
            sensor.set_option(RS2_OPTION_GAMMA, gamma);
            return true;
        case CAP_PROP_BRIGHTNESS:
            brightness = value;
            sensor.set_option(RS2_OPTION_BRIGHTNESS, brightness);
            return true;
        case CAP_PROP_CONTRAST:
            contrast = value;
            sensor.set_option(RS2_OPTION_CONTRAST, contrast);
            return true;
        case CAP_PROP_IOS_DEVICE_WHITEBALANCE:
            white_balance = value;
            sensor.set_option(RS2_OPTION_WHITE_BALANCE, white_balance);
            return true;
        default:
            DEBUG_ERROR_(__FILE__ << ", line" << __LINE__ << ": 尝试设置未定义变量。");
            return false;
    }
}

/**
 * @brief 读取图像
 *
 * @param color 彩色图像(CV_8UC3)
 * @param depth 深度图像(CV_16UC1)
 * @return true 读取成功
 * @return false 读取失败
 */
bool RSVideoCapture::read(Mat& color, Mat& depth)
{
    // 使用VideoCapture打开模组或视频
    if (__capture_status == OPENCV_CAPTURE || __capture_status == VIDEO_CAPTURE)
    {
        return cap->read(color);
    }
    // 使用深度相机或bag文件
    if (this->grab())
    {
        frameset alignedSet = __align->process(__frames);
        frame color_frame = alignedSet.get_color_frame();
        frame depth_frame = alignedSet.get_depth_frame();
        depth_frame = __hole_filling_filter.process(depth_frame);

        color = Mat(Size(width, height), CV_8UC3, (void*)color_frame.get_data(), Mat::AUTO_STEP);
        depth = Mat(Size(width, height), CV_16UC1, (void*)depth_frame.get_data(), Mat::AUTO_STEP);
    }
    else
    {
        // TODO (xicotl (videoc_s_fmt) failed last error: input/output error)的处理
        /*
            尝试重连时将原先的pipeline指针析构
            再新建个pipeline智能指针开启管道
        */
        DEBUG_ERROR_("offline!");
        reconnect();
        color.release();
        depth.release();
    }

    return (!color.empty() && !depth.empty());
}

/**
 * @brief realsense相机重连
 *
 * @return true 重连成功
 * @return false 重连识别
 */
bool RSVideoCapture::reconnect()
{
    // 重启前的标志位置位及管道停止
    if (cap != nullptr)
    {
        delete cap;
        cap = nullptr;
    }
    if (__is_pipeline_start)
    {
        __is_pipeline_start = false;
        __pipe.reset();
        __pipe = nullptr;
    }
    sleep(5);
    context cxt;
    device_list dev_list = cxt.query_devices();
    for (device dev: dev_list)
    {
        cout << "serial num: " << dev.get_info(RS2_CAMERA_INFO_NAME);
    }
    this->open();
    DEBUG_HIGHLIGHT_("------------------------------------- "
                     << "reconnect"
                     << " -------------------------------------");
    return true;
}

/**
 * @brief 配置录制record的config并重新启动pipeline
 */
void RSVideoCapture::BagWrite()
{
    __config.enable_record_to_file("../doc/videos/EngineerBag.bag");
    if (__is_pipeline_start && __capture_status == REALSENSE_CAPTURE)
    {
        __is_pipeline_start = false;
        __pipe->stop();
        this->open();
    }
    else
    {
        return;
    }
}

/**
 * @brief 析构函数
 *
 */
RSVideoCapture::~RSVideoCapture()
{
    if (cap != nullptr)
    {
        delete cap;
        cap = nullptr;
    }
    if (__is_pipeline_start)
    {
        __pipe->stop();
        __pipe.reset();
        __align.reset();
    }
}